%% Phase Detector
% 抽取信号的前 k 个幅度最大分量, 计算其幅值, 相位
k = 5;
%-- 信号生成
Fs = 40e3;
tSpan = 50e-3;
t = 0:1/Fs:tSpan;

freqCountRange = [4 5];
freqCount = randi(freqCountRange, 1);

freqRange = [20, 20e3];

freq = diff(freqRange) .* rand(1, freqCount) + freqRange(1);
amp = 10 .* rand(1, freqCount);
phi0 = 2*pi .* rand(1, freqCount);

y = zeros(size(t));
for i = 1:freqCount
	y = y + amp(i) .* sin(2*pi*freq(i) .* t + phi0(i));
end

%-- 幅频计算
sampleCount = length(y);
yFFT = fft(y);
yAmp = abs(yFFT/sampleCount); 				% 双侧振幅谱
yAmp = yAmp(1:floor(sampleCount/2) + 1);
yAmp(2:end-1) = 2 .* yAmp(2:end-1);			% 单侧振幅谱
yFreq = (0:sampleCount/2) .* (Fs/sampleCount); % 单侧频率

[amp_, freqIdx_] = maxk(yAmp, k); % 取前 k 个幅度最大的
freq_ = yFreq(freqIdx_);
phi0_ = 2*pi.*freq_;

%-- 相位计算
tcor = -tSpan:1/Fs:tSpan;
for i = 1:k
	yReference = amp_(i) .* sin(2*pi*freq_(i) .* t);
	
	[~, maxIndex_] = max(xcorr(yReference, y, 'normalized'));
	phi0_(i) = phi0_(i)*mod(tcor(maxIndex_), 1/freq_(i));
end

clc;
disp(sortrows([freq; amp; phi0]')');
% fprintf('freq'); disp(freq);
% fprintf('amp'); disp(amp);
% fprintf('phi0'); disp(phi0);
disp(' ');
disp(sortrows([freq_; amp_; phi0_]')');
% fprintf('freq'); disp(freq_);
% fprintf('amp'); disp(amp_);
% fprintf('phi0'); disp(phi0_);
